#ifndef __CONTROLLER_P2P_H_
#define __CONTROLLER_P2P_H_

#include <motion_control/controller/controller_base.h>

class ControllerP2P : public ControllerBase{
    public:
        ControllerP2P(boost::shared_ptr<PathPlanningBase> path_planner_ptr,
            boost::shared_ptr<VelocityPlanningBase> velocity_planner_ptr);
        ~ControllerP2P();

        /********************
         *  虚函数实现
         ********************/
        // 设置目标点数据
        void SetGoal(geometry_msgs::PoseStamped pose_stamp);
        // IDLE数据重置
        void ResetIdle();
        // 执行中预处理函数
        void ExecutingEntry();
        // 执行中处理函数
        void ExecutingRunning();
        // 执行中后处理函数
        void ExecutingExit();

    private:
        boost::shared_ptr<PathPlanningBase> globalPathPlannerPtr_;
        boost::shared_ptr<VelocityPlanningBase> velocityPlannerPtr_;
        // boost::shared_ptr<PlannerBridge> __planner_bridge_ptr;
        // boost::shared_ptr<SensorBridge> __sensor_bridge_ptr;
        Pose2D goalPose2D_;
        bool __rotate_start_direction;
        bool setGoalPose_;


};

#endif